import math
from matplotlib import pyplot as plt
import numpy as np

def sign(x):
    if (x>0):
        return 1
    else:
        return -1

def distance(p,pt):
    dx = pt[0] - p[0]
    dy = pt[1] - p[1]
    return math.sqrt(dx*dx +dy*dy)

class Pillar():
    def __init__(self,pos,tilt,len,altitude = 150):
        self.altitude = altitude
        self.pos_x = pos[0]
        self.pos_y = pos[1]
        self.tilt = tilt * math.pi / 180
        self.len = len

    def set_endpoint(self,p,pt):
        self.pos_x = (p[0] + pt[0])/2
        self.pos_y = (p[1] + pt[1])/2
        dx = pt[0] - p[0]
        dy = pt[1] - p[1]
        deg = math.atan2(dy,dx)
        half_pi = math.pi/2
        if deg > half_pi:
            deg -= math.pi
        elif deg < -half_pi:
            deg += math.pi
        self.tilt = deg
        self.len = math.sqrt(dx*dx + dy*dy)

    def get_endpoint(self):
        len = self.len/2
        deg = self.tilt
        dx = len * math.cos(deg) 
        dy = len * math.sin(deg)
        p = [self.pos_x + dx, self.pos_y + dy]
        pt = [self.pos_x - dx, self.pos_y - dy]
        return (p,pt)

    def get_through_path(self,n):
        len = self.len/2
        deg = self.tilt
        half_pi = math.pi/2
        # 旋转90度
        if deg > 0:
            deg -= half_pi
        else:
            deg += half_pi
        dx = len * math.cos(deg) 
        dy = len * math.sin(deg)
        path = []
        x_list = np.linspace(self.pos_x + dx, self.pos_x - dx, n)
        y_list = np.linspace(self.pos_y + dy, self.pos_y - dy, n)
        for i in range(n):
            path.append([x_list[i],y_list[i]])
        return path

    def norm_transform(self,x,y):
        deg = self.tilt
        sin_deg = math.sin(deg)
        cos_deg = math.cos(deg)

        #平移
        x -= self.pos_x
        y -= self.pos_y
        #旋转, 使x轴与杆平行
        tx = x * cos_deg + y * sin_deg
        ty = -x * sin_deg + y * cos_deg

        return (tx,ty)

    def inv_norm_transform(self,x,y):
        deg = self.tilt
        sin_deg = math.sin(deg)
        cos_deg = math.cos(deg)


        #旋转, 使x轴与杆平行
        tx = x * cos_deg - y * sin_deg
        ty = x * sin_deg + y * cos_deg

        #平移
        tx += self.pos_x
        ty += self.pos_y        

        return (tx,ty)

    # 计算一点与杆线段的距离
    def calc_distance(self,x,y):
        half_len = self.len / 2

        tx,ty = self.norm_transform(x,y)

        if (abs(tx)<= half_len):
            return abs(ty)
        else:
            sign_x = sign(tx)
            dx = tx - sign_x * half_len
            return math.sqrt(dx*dx + ty*ty)
        
    def redirect(self, bpos, r = 55):

        d1 = self.calc_distance(bpos[0][0], bpos[0][1])

        d2 = self.calc_distance(bpos[1][0], bpos[1][1])

        d = min(d1, d2)

        if d < r:
            if d1 <= d2:
                px, py = self.norm_transform(bpos[0][0], bpos[0][1])
            else:
                px, py = self.norm_transform(bpos[1][0], bpos[1][1])

            # 保证钝角趋近
            if abs(px) < self.len/2:    
                if py >= 0:
                    py -= r
                else:
                    py += r
                return self.inv_norm_transform(px, py)

        return (-1, -1)

            


    #判断两点连线是否与杆相交,相交返回 1,不相交返回 0 
    def if_cross(self,p1,p2):
        tx1,ty1 = self.norm_transform(p1[0],p1[1])
        tx2,ty2 = self.norm_transform(p2[0],p2[1])

        yy = ty1 * ty2
        
        if (yy > 0): #两点在x轴同侧
            return 0
        elif (yy == 0): #某点恰在x轴上
            return 1
        else:
            half_len = self.len / 2
            # x轴截距
            x_cut = tx1 - ty1 * (tx1 - tx2)/(ty1 - ty2)
            if(abs(x_cut) > half_len + 30):
                return 0
            else:
                return 1
            
    def if_near_ring(self,p1,p2):
        pj = Pillar([0,0], 0, 1)
        pj.set_endpoint(p1, p2)

        e1, e2 = self.get_endpoint()

        d1 = pj.calc_distance(e1[0], e1[1])
        d2 = pj.calc_distance(e2[0], e2[1])
        d = min(d1, d2)

        min_len = self.len / 2.5

        if d >= min_len:
            return 0
        else:
            return 1

            
    def draw(self):
        len = self.len/2
        deg = self.tilt
        dx = len * math.cos(deg) 
        dy = len * math.sin(deg)
        xl = [self.pos_x + dx, self.pos_x - dx]
        yl = [self.pos_y + dy, self.pos_y - dy]
        plt.plot(xl,yl)

            
class Circle():

    def __init__(self,pos,r):

        self.pos_x = pos[0]
        self.pos_y = pos[1]
        self.r = r



    def calc_distance(self,x,y):
        dx = self.pos_x - x
        dy = self.pos_y - y
        d = math.sqrt(dx*dx + dy*dy) - self.r
        if (d < 0):
            d = 0
        return d
    
    def if_cross(self,p1,p2):
        dx = p1[0] - p2[0]
        dy = p1[1] - p2[1]
        ox = (p1[0] + p2[0])/2
        oy = (p1[1] + p2[1])/2
        if (dx != 0):
            deg = math.atan(dy/dx)
        else:
            deg = math.pi/2
        sin_deg = math.sin(deg)
        cos_deg = math.cos(deg)

        half_len = math.sqrt(dx*dx + dy*dy)/2

        x = self.pos_x - ox
        y = self.pos_y - oy
        tx = x * cos_deg + y * sin_deg
        ty = -x * sin_deg + y * cos_deg

        if (abs(tx)<= half_len):
            d = abs(ty)
        else:
            sign_x = sign(tx)
            dx = tx - sign_x * half_len
            d = math.sqrt(dx*dx + ty*ty)

        d = d - self.r - 20

        if (d > 0):
            return 0
        else:
            return 1

        






